/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.AnalogModule;
import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import com.sun.squawk.VM;
import edu.wpi.first.wpilibj.GenericHID.Hand;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class TestSledCode extends IterativeRobot {

    DriverStation ds;
    Jaguar left, right;
    AnalogModule analog1, analog2;
    DigitalModule digital1, digital2;
    Joystick leftStick, rightStick;
    AxisCamera camera;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
	  ds = DriverStation.getInstance();
	  left = new Jaguar(4, 2);
	  right = new Jaguar(4, 1);
	  leftStick = new Joystick(1);
	  rightStick = new Joystick(2);
	  //try{
	  camera = AxisCamera.getInstance();
	  //}
	  //catch(AxisCameraException ace){
	  //	VM.println("!!!!!!!!!   camera error: "+"\n"+ace.getMessage());
	  // }
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
	  while (isAutonomous()) {
		if (isEnabled()) {
		    left.set(-.01);
		    right.set(.01);
		}
	  }
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
	  while (isOperatorControl()) {
		if (isEnabled()) {
		    if (leftStick.getRawButton(1) && rightStick.getRawButton(1)) {
			  boolean LB2, RB2, LB3, RB3;
			  LB2 = leftStick.getRawButton(2);
			  LB3 = leftStick.getRawButton(3);
			  RB2 = rightStick.getRawButton(2);
			  RB3 = rightStick.getRawButton(3);
			  double leftY = leftStick.getY();
			  double rightY = rightStick.getY();

			  double leftSpeed = leftY, rightSpeed = rightY;
			  if (LB2 || RB2) {
				if (LB2 && !RB2) {
				    leftSpeed = rightSpeed = leftY;
				}
				else if (!LB2 && RB2) {
				    leftSpeed = rightSpeed = rightY;
				}
			  } 
			  else if(LB3 || RB3){
				if(LB3 && ! RB3){
				    leftSpeed = leftY;
				    rightSpeed = -leftY;
				}
				else if(!LB3 && RB3){
				    leftSpeed = -rightY;
				    rightSpeed = rightY;
				}
			  }
			  left.set(leftSpeed);
			  right.set(-rightSpeed);
		    }
		    else{
			  left.set(0);
			  right.set(0);
		    }
		}
		else{
		    left.set(0);
		    right.set(0);
		}
	  }
	  left.set(0);
	  right.set(0);
    }
}

